import cv2
import numpy as np
import serial
import time

# img=cv2.imread('bend.jpg')
# img_hsv=cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
# cv2.imshow('img',img_hsv)

ser = serial.Serial("/dev/ttyAMA0", 9600)


capL = cv2.VideoCapture(5)  # 左侧摄像头
capR = cv2.VideoCapture(2)  # 右侧摄像头
cap_Color = cv2.VideoCapture(0)  # 颜色摄像头
cap_cnr = cv2.VideoCapture(3)  # 障碍物检测摄像头


# 赛道边界灰色阈值,HSV
edge_Lower = np.array([10, 30, 46])
edge_Upper = np.array([180, 43, 150])

# 障碍物检测，采用黑色阈值,BGR
cnr_Lower = np.array([0, 0, 0])
cnr_Upper = np.array([10, 10, 10])

# 颜色检测，格式为BGR
color_Lower = np.array([0, 0, 30])
color_Upper = np.array([5, 5, 130])
# brown  (5, 10, 30)->(25, 20, 55)
# red    (0,0,30)->(5,5,130)
# purple (40,0,15)->(45,5,30)

capL.set(3, 320)
capL.set(4, 240)

capR.set(3, 320)
capR.set(4, 240)

cap_Color.set(3, 640)
cap_Color.set(4, 480)

cap_Color.set(3, 640)
cap_Color.set(4, 480)

while 1:
    # 左侧赛道

    _, frameL = capL.read()
    frameL = cv2.GaussianBlur(frameL, (5, 5), 0)
    hsvL = cv2.cvtColor(frameL, cv2.COLOR_BGR2HSV)
    pixelL = hsvL.shape[0]*hsvL.shape[1]
    hsv_arrayL = np.array(hsvL)
    _, _, vL = cv2.split(hsvL)
    vL = vL.astype("uint8")
    _, thL = cv2.threshold(vL, 0, 255, cv2.THRESH_BINARY+cv2.THRESH_OTSU)
    thL = cv2.dilate(thL, None, iterations=2)
    thL = cv2.erode(thL, None, iterations=6)
    blackL = np.where(thL == 0)[0].shape[0]
    percent_blackL = blackL/pixelL

    if percent_blackL >= 0.25:
        flagL = '1'
        print(flagL)
        ser.write(flagL.encode())
    else:
        pass

    # 右侧赛道
    # start = time.process_time()
    retR, frameR = capR.read()
    frameR = cv2.GaussianBlur(frameR, (5, 5), 0)
    hsvR = cv2.cvtColor(frameR, cv2.COLOR_BGR2HSV)
    pixelR = hsvR.shape[0]*hsvR.shape[1]
    hsv_arrayR = np.array(hsvR)
    _, _, vR = cv2.split(hsvR)
    vR = vR.astype("uint8")
    _, thR = cv2.threshold(vR, 0, 255, cv2.THRESH_BINARY+cv2.THRESH_OTSU)
    thR = cv2.dilate(thR, None, iterations=2)
    thR = cv2.erode(thR, None, iterations=6)
    blackR = np.where(thR == 0)[0].shape[0]
    percent_blackR = blackR/pixelR

    if percent_blackR >= 0.25:
        flagR = '2'
        print(flagR)
        ser.write(flagR.encode())
    else:
        pass

    # 如果左右都未检测到边界，则通过串口发送0
    # if (len(cntsL) == 0 & len(cntsR) == 0):
    #     flagNULL = '0'
    #     print(flagNULL)
    #     ser.write(flagNULL.encode())

    # 颜色检测，检测到发送字符'b'
    _, frame_Color = cap_Color.read()
    frame_Color = cv2.GaussianBlur(frame_Color, (5, 5), 0)
    mask_Color = cv2.inRange(frame_Color, color_Lower, color_Upper)
    mask_Color = cv2.erode(mask_Color, None, iterations=2)
    mask_Color = cv2.GaussianBlur(mask_Color, (3, 3), 0)
    res_Color = cv2.bitwise_and(frame_Color, frame_Color, mask=mask_Color)
    cnts_Color = cv2.findContours(
        mask_Color.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2]
    if len(cnts_Color) > 0:
        cnt = max(cnts_Color, key=cv2.contourArea)
        (x, y), radius = cv2.minEnclosingCircle(cnt)
        cv2.circle(frame_Color, (int(x), int(y)),
                   int(radius), (255, 0, 255), 2)
        # print(int(x),int(y))
        print('b')
        flag = 1
        p = 'b'
        ser.write(p.encode())

    else:
        pass

    # 障碍物角点检测，检测到发送字符'c'
    _, frame_cnr = cap_cnr.read()
    frame_cnr = cv2.GaussianBlur(frame_cnr, (5, 5), 0)
    mask_cnr = cv2.inRange(frame_cnr, cnr_Lower, cnr_Upper)
    mask_cnr = cv2.erode(mask_cnr, None, iterations=2)
    mask_cnr = cv2.GaussianBlur(mask_cnr, (3, 3), 0)
    res_cnr = cv2.bitwise_and(frame_cnr, frame_cnr, mask=mask_cnr)
    cnts_cnr = cv2.findContours(
        mask_cnr.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2]
    if len(cnts_cnr) > 0:
        cnt = max(cnts_cnr, key=cv2.contourArea)
        (x, y), radius = cv2.minEnclosingCircle(cnt)
        cv2.circle(frame_Color, (int(x), int(y)),
                   int(radius), (255, 0, 255), 2)
        # print(int(x),int(y))
        print('c')
        flag = 1
        p = 'c'
        ser.write(p.encode())

    else:
        pass

    cv2.imshow('frameLeft', frameL)
    cv2.imshow('frameRight', frameR)
    cv2.imshow('frameColor', frame_Color)
    cv2.imshow('frameCorner', frame_cnr)

    if cv2.waitKey(5) & 0xFF == 27:
        break

capL.release()
cv2.destroyAllWindows()
